#By: mengfei - 周日 1月 15 2023
import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from umotor import UMOTOR
from pid import PID
from button import BUTTON
from centroidtracking import CentroidTracking
from pycommon import PYCOMMON
common=PYCOMMON()
objtraking=CentroidTracking(maxDisappeared=5) #目标追踪算法，质心追踪maxDisappeared表示最大丢失5帧代表目标消失
button=BUTTON()  #声明按键，梦飞openmv只有一个按键D8，因此直接内部指定了按键
objects = list()
last_objects=objects
lcd.init(type=2,width=240,height=320)
lcd.set_direction(2)

pan_servo=Servo(3)    #左右控制PD12
tilt_servo=Servo(4)  #上下控制PD13
pan_start_angle=0.0
tilt_start_angle=-20.0 #朝上更容易看到人脸

pan_angle=pan_servo.angle(pan_start_angle)    #上下运动控制设置一个初始角度
tilt_angle=tilt_servo.angle(tilt_start_angle)   #左右舵运动控制，初始角度

pan_pid = PID(p=0.09, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.07, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID

flag_lost =0
object_s =0

#标签参数
# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。

# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算

# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）

# c_x 是图像的x中心位置
# c_y 是图像的y中心位置
#OV2640,广角摄像头内参
#openmv的实际距离 k =√(tx^2+ty^2+tz^2)
f_x = (3.6 / 3.590) * 240 # 默认值
f_y = (3.6 / 2.684) * 160 # 默认值
c_x = 240 * 0.5 # 默认值(image.w * 0.5)
c_y = 160 * 0.5 # 默认值(image.h * 0.5)

def degrees(radians):
    return (180 * radians) / math.pi

#功能： 寻找最大的目标，计算方式为像素面积排序
#输入： 链表objects，坐标X,坐标y,像素宽,像素高
#输出： 最大目标元组
def find_max_object(objects):
    max_size=0
    for object in objects:
        if object[2]*object[3] > max_size:
            max_object=object
            max_size = object[2]*object[3]
    return max_object

#功能：目标距离计算
#输入：目标宽度或直径，单位：像素值
#输出：距离 mm
#note:不同的镜头焦距和摄像头芯片对应的距离参数不同，具体可配合超声波测距模块进行距离参数标定
def obj_distance(obj_Lm):
    distance= 10000/obj_Lm   #不同得摄像头测距参数需要标定
    return int(distance)


#功能： 小球颜色识别
#输入： 图像
#输出： object_s,max_blob
def tag_detect(img):
    DIS_RATE=27000          #测距离系数，使用图像像素多少估算距离
    #全局变量
    global last_objects,objects,flag_lost
    object_s=0
    max_blob=None
    if last_objects:  #如果上一张图像中找到了色块，就在色块周边扩展，然后在扩展后的ROI区域进行寻找色块，这样有利于追踪
         for b in objects:
            x1 = b[0]-12
            y1 = b[1]-12
            w1 = b[2]+21
            h1 = b[3]+21
         roi2 = (x1,y1,w1,h1)
         objects = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y,roi=roi2) # 默认为TAG36H11
         last_objects = objects
    else:       #如果没有找到色块，就重新计算全图的色块，并将全图色块信息保存到上一张图像上，这样下一次就可以再进行ROI区域寻找色块
        objects = img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y) # 默认为TAG36H11
        last_objects = objects
    xrect=[]
    if objects:
        for d in objects :
            xrect.append(d.rect())
    objects=objtraking.update(xrect)
    #step1 有目标，就进行追踪和抓取操作
    if objects:  #如果找到了色块，就计算最大的色块位置，并做舵机追踪
        flag_lost=0
        object_s=0
        max_blob = common.find_max_object(objects)
        #色块的位置和大小需要满足图像本身大小的要求，以免越界
        object_s=DIS_RATE/(max_blob[2]*2) #计算距离
        img.draw_rectangle((max_blob[0],max_blob[1],max_blob[2],max_blob[3]))
        cx=int(max_blob[0]+max_blob[2]/2)
        cy=int(max_blob[1]+max_blob[3]/2)
        img.draw_cross(cx, cy) # cx, cy画中心点
        img.draw_string(cx,cy, "%.2f mm"%(object_s))  #显示目标的距离参数
    return object_s,max_blob

#功能： 自动找球控制
#输入： 图像，小球距离,最大面积的小球元组
#输出： 无
def apriltag_detect_traking(img,mode=0):
    global flag_lost
    object_s=0
    object_s,max_blob=tag_detect(img)
    if object_s>0:
        #step1 计算目标距离图像中心的偏差
        pan_error=0
        flag_lost=0
        pan_error = img.width()/2-(max_blob[2]/2+max_blob[0])   #左右控制的偏差
        tilt_error =img.height()/2-(max_blob[3]/2+max_blob[1]) #上下控制的偏差
        pan_output=pan_pid.get_pid(pan_error,1)/2   #计算PID参数
        tilt_output=tilt_pid.get_pid(tilt_error,1)/2  #计算PID参数
        pan_angle=pan_servo.angle()+pan_output
        tilt_angle=tilt_servo.angle()-tilt_output
        if pan_angle>=60:   pan_angle=60
        if pan_angle<=-60:   pan_angle=-60
        if tilt_angle>=60:   tilt_angle=60
        if tilt_angle<=-60:   tilt_angle=-60
        pan_servo.angle(pan_angle)
        tilt_servo.angle(tilt_angle)
    else :
        flag_lost=flag_lost+1
        if flag_lost>5:#连续5帧没有
            flag_lost=0
            pan_angle=pan_servo.angle(pan_start_angle)    #上下运动控制设置一个初始角度
            tilt_angle=tilt_servo.angle(tilt_start_angle)   #左右舵运动控制，初始角度


##############################摄像头初始化部分#####################
sensor.reset() # Initialize the camera sensor.
#sensor.__write_reg(0x11,0x81) #图像传感器的帧率控制，80为30帧，81为15帧
sensor.set_pixformat(sensor.GRAYSCALE) # use RGB565.彩色图像
sensor.set_framesize(sensor.HQVGA)
#sensor.set_windowing((320, 320))
sensor.set_hmirror(True) #水平镜像，暂时不用
sensor.set_vflip(True)  #垂直镜像，根据摄像头的安装位置调整
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
sensor.skip_frames(10) # Let new settings take affect.

click_timer=time.ticks() #计时参数
clock = time.clock()
while(True):
    clock.tick()
    img = sensor.snapshot()
    apriltag_detect_traking(img)
    print(clock.fps(), "fps", end="\n\n")
    lcd.display(img)
    if button.state():
        click_timer=time.ticks()          #开始计时
        while button.state():  pass       #等待按键抬起
        if time.ticks()-click_timer>2000: #按键时长超过2s
            break                         #循环退出，回到主界面
    else :
        click_timer=time.ticks()#计时更新



